/******************************************************************
* PS2手柄按键测试程序
* 
* 使用0.96寸 OLED SPI 屏显示
* 
* 更新时间
*   2020/2/8 测试通过
******************************************************************/


#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
#include "PS2X_lib.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//---------------------------------------
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_MOSI   5
#define OLED_CLK   6
#define OLED_DC    3
#define OLED_CS    2 //悬空
#define OLED_RESET 4
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT,
                         OLED_MOSI, OLED_CLK, OLED_DC, OLED_RESET, OLED_CS);

#define MySetCursor(x,y) display.setCursor(x*6, y*8) //显示坐标 (0-20,0-7)
//---------------------------------------
                 
PS2X ps2x; // create PS2 Controller Class

int error = 1;

void setup() {
  Serial.begin(115200);
  
  /**********显示屏初始化**********/
  //注意需要有1200字节左右的动态内存空余作为屏幕的buffer
  if (!display.begin(SSD1306_SWITCHCAPVCC)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;); // Don't proceed, loop forever
  }
  //display.clearDisplay();
  display.display();
  delay(500); // Pause for 2 seconds
  display.clearDisplay();

  /**********手柄初始化**********/
    do{
        error = ps2x.config_gamepad(13, 11, 10, 12, false, true);
        if(error == 0){
          Serial.print(F("Found Controller, configured successful "));
          break;
        }
        else if(error == 1)
            Serial.println(F("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips"));
           
        else if(error == 2)
            Serial.println(F("Controller found but not accepting commands. see readme.txt to enable debug"));
        
        else if(error == 3)
            Serial.println(F("Controller refusing to enter Pressures mode, may not support it. "));
        
        delay(100);
     }while (1);

    for (size_t i = 0; i < 50; i++)
    {
        ps2x.read_gamepad(false, 0);
        delay(10);
    }

}
void loop() {
  if(error == 1) //skip loop if no controller found
    return; 
  ps2x.read_gamepad(false, 0);

  //L1
  press_or_not(PSB_L1);
  MySetCursor(2, 1);
  display.println(" L1 ");
  //L2
  press_or_not(PSB_L2);
  MySetCursor(2, 0);
  display.println(" L2 ");
  //R1
  press_or_not(PSB_R1);
  MySetCursor(14, 0);
  display.println(" R1 ");
  //R2
  press_or_not(PSB_R2);
  MySetCursor(14, 1);
  display.println(" R2 ");

  //pad_up
  press_or_not(PSB_PAD_UP);
  MySetCursor(4, 3);
  display.println("Y");
  //pad_down
  press_or_not(PSB_PAD_DOWN);
  MySetCursor(4, 5);
  display.println("Y");
  //pad_left
  press_or_not(PSB_PAD_LEFT);
  MySetCursor(3, 4);
  display.println("Y");
  //pad_right
  press_or_not(PSB_PAD_RIGHT);
  MySetCursor(5, 4);
  display.println("Y");

  //🔺
  press_or_not(PSB_TRIANGLE);
  MySetCursor(16, 3);
  display.println("Y");
  //◉
  press_or_not(PSB_CIRCLE);
  MySetCursor(17, 4);
  display.println("Y");
  //■
  press_or_not(PSB_SQUARE);
  MySetCursor(15, 4);
  display.println("Y");
  //X
  press_or_not(PSB_CROSS);
  MySetCursor(16, 5);
  display.println("Y");
  
  //select
  press_or_not(PSB_SELECT);
  MySetCursor(8, 4);
  display.println("Y");
  //start
  press_or_not(PSB_START);
  MySetCursor(12, 4);
  display.println("Y");

  //注意一位或两位数时旧的第三位不会自动清除
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  MySetCursor(6, 6);
  display.println( ps2x.Analog(PSS_LX) );
  
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  MySetCursor(6, 7);
  display.println( ps2x.Analog(PSS_LY) ); 
  
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  MySetCursor(12, 6);
  display.println( ps2x.Analog(PSS_RX) );
  
  display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  MySetCursor(12, 7);
  display.println( ps2x.Analog(PSS_RY) ); 
  
  display.display();

}

void press_or_not(uint16_t button)
{
  if (ps2x.Button(button))
  {
    display.setTextColor(SSD1306_BLACK, SSD1306_WHITE);
  }
  else
  {
    display.setTextColor(SSD1306_WHITE);
  }
}
